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Abstract 

An efficient 0(mN) algorithm for dynamic simulation of simple closed-chain robotic mechanisms will 
be presented in this paper, where m is the number of chains, and N is the number of degrees of 
freedom for each chain. It is based on computation of the operational space inertia matrix (6 X 6) 
for each chain as seen by the body, load, or object. Also, computation of the chain dynamics, when 
opened at one end, is required, and the most efficient algorithm is used for this purpose. Parallel 
implementation of the dynamics for each chain results in an O(N) + 0(log 2 m + l) algorithm. 

I. Introduction 

Recently, there has been an increasing interest in robotic systems with multiple chains forming simple 
closed kinematic loops. Such systems of interest in space robotics applications include multilegged 
vehicles, multiple manipulators, and dexterous hands. Each is characterized by multiple chains of 
links (legs, arms, or fingers) in support of a body, load, or object. Real-time simulation of these 
systems is important for remote operation, but difficult to achieve at present. An even greater 
challenge to the computational engineer is that of super-real-time simulation , that is, planning seconds 
of motion in milliseconds. This has been shown to be of value in the control of a multilegged vehicle 
when predicting the action of the present control to ensure safety and stability along a planned 
trajectory. 

The fundamental goal of this paper is the development of an efficient algorithm for the dynamic sim- 
ulation of the time- varying topological systems discussed above. Previous researchers have presented 
algorithms for these and similar configurations based on equation augmentation [1], constraint prop- 
agation [2], and recursive computation [3,4], but these methods are often difficult to apply and/or 
computationally inefficient. The new simulation algorithm derived here makes use of efficient com- 
putations for the individual supporting chains to produce an efficient simulation method for the 
complete robot system. The dynamic properties of each chain are described in a simple, physically 
understandable manner, which facilitates the straightforward analysis of the combined dynamics of 
the entire mechanism. 

Multiple chain robotic systems can take many forms, some of them quite complex. Simple closed- 
chain mechanisms are a subset of multiple chain systems with specific structural characteristics. The 
structure of a simple closed-chain mechanism is characterized by m actuated chains which support a 
single common reference member [1]. A supporting chain is identified as an independent functional 
unit in the closed chain system which has two ends, each terminated by a single link. Each chain 
may have an arbitrary number of links and degrees of freedom, and closed kinematic loops within 
a chain are permitted. The removal of the reference member breaks the closed loops formed by the 
multiple chains. 
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Figure 1: Example of a Type 0 Simple Closed-Chain Mechanism 



Figure 2: Example of a Type 1 Simple Closed-Chain Mechanism 


There are two basic types of simple closed-chain mechanisms called Type 0 and Type 1, respectively 
[1]. These two types are defined based on the nature of the interactions which occur between the 
links of each chain and the reference member or support surface. Figure 1 illustrates a typical Type 

0 mechanism which may be used to model multiple manipulators or dexterous hands. Note that the 
support surface, shown here as a fixed inertial frame for a multiple manipulator configuration, might 
also represent the moving “palm” of a dexterous hand. In either case, for a Type 0 mechanism, the 
base link of each chain is connected to the support surface by an actuated joint structure, while the 
last link interacts with the reference member through an unpowered contact. Figure 2 illustrates a 
Type 1 simple closed-chain mechanism which may be used to model multilegged vehicles. For a Type 

1 mechanism, the last link of each chain interacts with the support surface through an unpowered 
contact, while the base link is connected to the reference member by an actuated joint structure. For 
both Type 0 and Type 1 mechanisms, the reference member (object, load, or body) is numbered 0, 
while the chains are numbered arbitrarily from 1 to m. Chain k (k = l,...,m) has TV* degrees of 
freedom, where JV* may be less than, equal to, or greater than 6. 

In order to apply the same algorithm to both types in this work, the support surface will be considered 
to act as the “base” of each chain. We will refer to the terminal link which interacts with the support 
surface as link 1, and the terminal link which interacts with the reference member will be called the 
last link or end effector (link N ). The far end of link N is the “tip” of the chain. The interactions 
and connections which occur between bodies or links in the system (including those at the support 
surface and at the reference member) will be described using the general joint model of [5,6]. This 
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includes both powered joint structures and unpowered contacts. The motion of the support surface 
is assumed to be known. 

In this paper, an 0{mN ) recursive algorithm for the dynamic simulation of simple closed-chain 
mechanisms is derived for m chains with N degrees of freedom each. The algorithm is based on the 
efficient computation of the (6 X 6) operational space inertia matrix [7] for each chain as seen by the 
body, load, or object. The operational space inertia matrix, A, may be used to obtain the net effect 
of the chain dynamics at its tip. The computation of the chain dynamics when the chain is open 
at one end is also required, and the most efficient algorithm is used for this purpose. Given O(N) 
algorithms for these two fundamental computations for each chain [4, 5, 8, 9], an O(mN) algorithm for 
the simulation of the entire multiple chain system is formulated. 

In the next section, the notational and modelling conventions used in the formulation of the new 
algorithm are summarized. In the third section, the dynamic properties of the individual supporting 
chains and the common reference member are discussed, and the appropriate dynamic equations 
are developed. The operational space inertia matrix and the open-chain dynamics of each chain 
are of special significance in this discussion. In the fourth section, the O(mN) dynamic simulation 
algorithm for simple closed-chain mechanisms is derived. The final algorithm is presented as a series 
of five steps, which are summarized in a convenient tabular form. The computational requirements 
of the new algorithm, including parallel implementation considerations, are presented in the t 
section. Finally, the results of this work are summarized and some overall conclusions are given in 

the final section. 


II. Notation 

Many of the notational conventions used in this paper are based on concepts introduced by Roberson 
and Schwertassek in [6] and used by Brandi, Johanni, and Otter in [5], They are similar in many 
ways to those described by Featherstone in [9] for robot dynamics, although there are a few minor 
differences. As in each of these, spatial notation will be used to develop the dynamic equations for 
the chains and reference member. With spatial notation, velocity, acceleration, and force vectors are 
all 6 X 1 column vectors, where each incorporates the appropriate linear and angular components. In 
this paper, the spatial velocity of the reference member, v 0 , is written: 

Vo = [ (wo)x ( w o)y (wo) 2 (*>o)x (uo)y ( v o)z ] » ^ 

where (w 0 )i, (^o)y, and (w 0 )» are the components of the angular velocity of the reference member 
about x, y , and z, respectively, usually resolved in the reference member frame (frame 0) or an inertial 
coordinate system. The three components, (t*)„ («o) y , and («o)z, represent the linear velocity of the 
coordinate origin of frame 0. Similarly, the spatial acceleration of the reference member is expressed 


as: 


ao = [ (ao)i («o)y ( a o)z ( a o)x (®o)y ( a o)z ] » 


( 2 ) 


where the individual components now correspond to resolved angular and linear acceleration vectors. 
Spatial force vectors have a corresponding structure: 

f* = [ (nk)x (ttfc)y ( ra *)z (fk)x (fk)y (fk)z ] > ^ 

where in this case, f fc will be used to represent the spatial force exerted on the reference member 
by chain fc. The first three components, ( n k ) x , ( n k ) y , and (n*) 2 , represent the elements of a t ree- 
dfmensional moment vector, while (f k ) x , (/*)„ and (/*). are the elements of a three-dimensional 

force vector. 
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In general, the transformation of a spatial velocity or acceleration vector from one coordinate system 
to another one may be accomplished by the following spatial multiplication [9]: 

J p = % *p, ( 4 ) 

where *p is the vector expressed with respect to the ith coordinate system, Jp is the same vector 
expressed with respect to the yth coordinate system, and is the 6x6 spatial transformation 
matrix. This spatial transformation is defined as follows: 


*Xi = 


j A i 0 

J A,bT i A . 


(5) 


where •'A, is the 3 X 3 rotation transformation between the two coordinate systems, and b_, is the 
3x1 position vector from theprigin of frame i to the origin of frame j , with components expressed 
in frame i. The 3x3 matrix, bj, is an anti-symmetric matrix defined by the rule: 


c = 


0 — c 3 c 2 

C3 0 -Cl 

-c 2 Cl 0 


( 6 ) 


In spatial notation, inertia matrices are also expressed as 6 X 6 matrices. An inertia matrix may be 
defined for each individual link of a chain, as well as the reference member, in its own corresponding 
coordinate system. For the reference member, this matrix, I 0 , is represented as follows: 


Io = 


lo ho 

ho Mo 


(7) 


where Mo is a 3 X 3 diagonal matrix of the mass of the reference member, and Io is the 3x3 moment 
of inertia tensor at the origin of coordinate frame 0. The matrix I 0 is symmetric and positive definite, 
but not necessarily diagonal. The 3x3 matrix, h 0 , is equal to m 0 s 0 , where m 0 is the mass of the 
reference member, and s 0 is the position vector of the center of gravity of the reference member from 
the coordinate origin of frame 0. Because I 0 and s 0 are defined in coordinate system 0, the matrix 
Io is constant. 


To include general joints and contacts with multiple degrees of freedom in a multibody system, an 
extended model of the interconnections and interactions between individual bodies of that system is 
required. In this paper, the general joint model of Roberson and Schwertassek [6] is used for this 
purpose. This model is also used by Brandi, Johanni, and Otter in [5], 


Briefly, each interconnection and/or interaction between two bodies in a simple closed-chain mecha- 
msm hereafter referred to as a “general joint” , is described in terms of two orthogonal vector spaces 
0 and 4> ■ The matrix 4> is of dimension 6 X n, where n represents the number of degrees of freedom 
of the general joint, and it has full column rank. It represents the free modes of the joint and its 

columns make up a bas ! s f ° r th f free vector s P ace - We will refer to 4> as the motion space of the 

modi f ». wUc J is 6 x ( 6 ~ and also of full rank, represents the constrained 

Rmfc !a f ^ J J ° 1 n t ' Tt “ orth °g° nal to <t>> and may be called the constraint space of the joint. 
Both <p and <f> are usually resolved m the joint frame, and thus, they are both constant. 


III. Dynamic Properties of Individual Chains and Reference Member 


Each chain in a simple closed-chain mechanism is governed by the dynamic 
a single chain. For chain k, k = 1, . . . , m, these are: 


equations of motion for 
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Tfe = Hfc qjfe + Cfc qfc + Gfc + J k f 


( 8 ) 


where 


Tk 

<hfc,<u,qfc 

H fc 

c k 

Gk 

Jk 


Nk X 1 applied general joint torque/force vector, 

Nk X 1 general joint position, rate, and acceleration vectors, 
N k X N k joint space inertia matrix, 

Nk x Nk centripetal/Coriolis matrix, 

Nk x 1 gravity vector, 

6 X Nk Jacobian matrix, 


and f* is the (6x1) spatial force vector exerted by chain k on the reference member. 


Note that H&, C k , G k , and J* are functions only of the general joint position and rate vectors, q* 
and qjt, respectively. Recall also that the “base” of each chain is the support surface, and the “tip” 
of each chain touches the reference member. The components of q* and r fc correspond to the general 
joints of each chain, starting with the joint between link 1 and the support surface and ending with 
the joint preceding link N. The basic unknowns in Eq. (8) are the general joint accelerations, q*, 
and the components of the force vector, f*, in the constrained directions of the general joint at the 
tip of chain k. 


We may use the dynamic equations of motion to partition the joint acceleration and spatial tip 
acceleration vectors of each chain into the difference of two terms, one known and one unknown. For 
each chain, we may write [11]: 

ilk = (<U) ope n - (H* ^f*, (9) 

= (tylc)open (^®) 

where (q*) op en is the vector of joint accelerations for chain A; in an open, unconstrained configuration 
(f* = 0), and fl* is a function of the joint positions for chain k. Likewise, for x*, the tip acceleration 
for each chain: 


X fe = (Xfc)ope»-(J*Hj 1 jJ , )lfc l (11) 

= (Xfc)open - (12) 

where (x*)o P en is the spatial tip acceleration vector for chain A; in an open, unconstrained configu- 
ration, and Aj^ 1 is the inverse operational space inertia matrix for chain A:, defined at the tip of the 
chain [7,11]. 


The open-chain terms, (q*)o pcn and ( x*) opcn , are completely defined for each chain given the present 
state joint positions and rates, q* and q*, the applied joint torques/forces in the free directions, r*, 
and the motion of the support surface. An appropriate open-chain Direct Dynamics algorithm may 
be used to calculate these terms. The 0(N) recursive algorithms of [5,10] are very efficient for this 
computation, and the linear order of computation is highly desirable. Because the joint positions 
are known, 17* and AjjT 1 are also defined. Efficient algorithms for 17* and A^ 1 for a single chain are 
derived in [11], including an O(N) recursive algorithm which is the most efficient for N > 6. 


The dynamic behavior of the reference member may be described using a spatial force balance 
equation for that body. The sum of the spatial forces exerted by each chain on the reference member 
and any other external spatial forces (including gravity) are equal to the resultant force on the 
reference member. Using spatial notation, we may write the force balance equation as follows: 
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( 13 ) 


Fo = £°f* + go, 

Jt=l 

where 

Fq = 6x1 resultant spatial force vector applied to the reference member, 

°fjt = 6x1 spatial force vector applied by chain k to the reference member, 

and 

g 0 = 6x1 external spatial force vector applied to the reference member 

(including gravity). 

Each force term in Eq. (13) is defined with respect to the coordinate frame attached to the reference 
member (frame 0). Applying the basic Newton-Euler equations, we may also write the resultant 
vector, Fo, as follows: 

Fo — Ioa 0 + v 0 x Io v 0 , 

= Io a o + bo, 

where 

I o = 6x6 spatial inertia of the reference member, 

ao = 6x1 spatial acceleration of the reference member, 
vq = 6x1 spatial velocity of the reference member. 

Both vo and ao refer to the motion of the coordinate origin of frame 0. The spatial inertia matrix, 
Io, is also defined at this point, and it is known and constant. Because Vo is given for the present 
state, the velocity-dependent term, b 0 , is known. If we combine Eqs. (13) and (15), we finally obtain 
the following dynamic equation for the reference member: 

m 

5^ °f!fc + go = Ioao + bo- (16) 

&= l 

In this equation, the basic unknowns are ao and the components of °fit in the constrained directions 
of the general joint at the tip of chain k . 

IV. Multiple Chain Algorithm 

In developing an efficient algorithm for the dynamic simulation of simple closed-chain mechanisms, 
we are naturally led to consider the relationship between the physical structure of the robotic sys- 
tem and the computational structure of the desired algorithm. Intuitively, it seems apparent that 
the structural parallelism present in a simple closed-chain mechanism should lead to computational 
parallelism in the solution of the Direct Dynamics problem for that mechanism. 

More specifically, in a simple closed-chain mechanism, the m actuated chains act on the reference 
member in parallel, and their motion is coupled with that of the reference member. If the reference 
member is removed, the chains may function independently. Computationally, the physical removal 
of the reference member corresponds to solving for the forces which are exerted on it by each chain. 
Once these forces are known, the system is equivalent to a group of independent chains with known 
tip forces. The joint accelerations may then be computed for each chain separately. Given enough 
processors (one per chain), the computations for each chain may be carried out in parallel. 


(14) 

(15) 
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We will illustrate the basic methodology of the new simulation algorithm by first examining a simple 
special case. Consider m manipulators rigidly grasping a common object. Each manipulator has six 
degrees of freedom, and no chain is in a singular position. For simplicity, we will express all of the 
relevant equations in absolute coordinates. Because each chain tip is rigidly attached to the reference 
member, we may write: 


xjb = a 0 (17) 

for each chain A;, k = Thus, the operational space dynamic equation for each chain, as 

given in Eq. (12), takes the form: 

&o — (xfc) 0 p en — f/p. (18) 

Because no chain is in a singular position, and each chain has a full six degrees of freedom, Ajt is 
defined [11]. We may, therefore, solve for the spatial tip force exerted by chain k on the reference 
member, ft, as follows: 

ft = [(xt)open a 0 ] • (19) 

With this equation we have established an explicit relationship between the spatial tip force, f*, and 
the spatial acceleration of the reference member, ao. This expression may be used in the reference 
member dynamic equation, given in Eq. (16), to obtain: 

m 

J2 Ak [(**Wn - a 0 ] = Io ao + bo - go- (20) 

k=l 


The only unknown in Eq (20) is ao, the spatial acceleration of the reference member. Collecting 
terms, we may write: 


m 

io + k 


*=1 


a 0 = 


m 

£a k (Xjfc)open 
-fc= 1 


bo + go • 


( 21 ) 


We may now solve for ao from this linear system of algebraic equations using any linear system 
solver. Note that the characteristic matrix is just the sum of the operational space inertia matrices 
of the individual chains and reference member, and is only 6x6. With ao known, we may also 
solve explicitly for the spatial tip force f*, k = 1 using Eq. (19). Thus, the motion of the 

reference member and the spatial force exerted at the tip of each chain are completely defined. The 
simple closed-chain mechanism is effectively decoupled. Each manipulator may now be treated as 
an independent chain with a known spatial tip force. The joint accelerations for each chain may be 
computed separately using an appropriate Direct Dynamics algorithm and then integrated to obtain 
the next state. 


The method outlined above is quite straightforward. Of course, the illustrated example represents a 
special case. We will now develop a similar approach for a general simple closed-chain mechanism. 
Consider a mechanism with m chains, each with an arbitrary number of degrees of freedom, N. The 
interaction between each chain tip and the reference member is arbitrary and will be modelled using 
the general joint model of [6]. To begin, we will derive an explicit relationship between the spatial 
acceleration of each chain tip and the spatial acceleration of the reference member. The spatial 
acceleration of the tip of chain k is denoted by x*,. The relative spatial acceleration between the tip 
of chain k and the reference member, x£, resolved in the orthogonal vector spaces of the general joint 
between them, may be written: 

Xfc = (4>)k<* k + ( <t> c )k<x c k , 


79 

C-5k 


(22) 


where (<£)* and (4> c )k are the motion space and constraint space of the general joint at the tip of chain 
respectively. The quantities a and a c k are the corresponding components of relative acceleration 
in the free and constrained directions. For each chain, (<£)*, (<t> c )kj and a£ are known, while a % is 
unknown. The sum of x* and x£ is just the spatial acceleration of the reference member on the far 
side of the general joint between it and chain fc, a£. Thus, we may write: 

a£ = Xfc + x;, (23) 

= x* + (<£)* a* + (0 c )*a£. (24) 

We may also express in terms of the spatial acceleration of the reference member, ao, as follows: 

a* = X*ao + Co*, (25) 

where X§ = ^Xq is the spatial transformation between coordinate frame 0 and the coordinate frame 
associated with the general joint at the tip of chain k . The quantity £o is the 6x1 bias acceleration 
vector which is a function of the position and spatial velocity of the reference member. Because the 
present state of the entire system is given, both Xq and Co & Te known. 

Equating the two expressions above for a£, we obtain the following: 

X* + (<t>)k «fc + ( 4> c )k a fc — Xq ao + Co*- (26) 

This equation matches the spatial accelerations at the coupling point between chain k and the refer- 
ence member, giving an explicit relationship between x* and ao when the coupling is arbitrary. The 
basic unknowns in Eq. (26) are x*, a*, and ao. All other vectors and matrices may be computed 
rather simply from the initial information given for the simulation problem. 

To decouple the chains and the reference member, we need an explicit mathematical relationship 
between the spatial force exerted by chain k on the reference member, ffc, and the spatial acceleration 
of the reference member, a 0 . Equation (26) relates the spatial acceleration of the reference member 
and the spatial acceleration of the tip of chain k. We may eliminate a*, the unknown components 
of the relative acceleration, by projecting Eq. (26) onto the constraint space of the corresponding 


general joint as follows: 

(4> c )l [x fc + (<f>) k «fc + {4> c )k <*%) = ml [x$ ao + Co*] • (27) 

By definition [11]: 

{4> c )l Wfc = 0, (28) 

and . 

(<t> c )k(<f> c )k = 1 - ( 29 ) 

Thus, we may write: 

ml *fc + a fc = ml [x§ ao + Co*] • (30) 

Equation (12) defines x* in terms of the desired force vector, f/t. If we combine Eqs. (12) and (30), 
we obtain: 

ml [(Xfc)open - A; 1 **] = ml [x$ ao + Co*] - «fc, (31) 


or 
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(32) 


[(40* a; 1 ] f fc = 


a k ~ (40* Co + (40if (x*)open] ' [($ )k -^o] ®<>- 


The first bracketed term on the right side of Eq. (32) is completely known. The only unknowns in 
this equation are the constraint components of the force vector, f*, and the spatial acceleration, ao- 
We may now pursue an explicit relationship between these two vectors. 


Like the relative acceleration vector, f* may also be resolved in the orthogonal vector spaces of the 
general joint at the tip of chain k as follows: 

f fc = (4>) k h fc + ( 4> c ) k K, (33 ) 

where is the vector of known force components in the free directions, and h£ is the vector of 
unknown force components in the constrained directions. Combining Eqs. (32) and (33), we obtain: 

(</0* Aj~ 1 [(4>)k hfc + (4> C )k h£] = [ajfc - (40* Co + (40* (Xfc)open] 

- [(4> c )l Xq] ao. (34) 

If the spatial acceleration of the reference member is known, we may find an explicit solution for the 
unknown force components at the tip of chain k from the following set of linear algebraic equations. 

[(<£ C )* AjO(40fc] h£ = - (40* [Co ~ (^fc)open + A fc (<^)* hfc|| 

-[(40**§] ao, (35) 

= Sfc — [(<^ c )fc Xq] ao, (36) 

where S* is known. Even when ao is unknown, we may still find a solution for h£ in terms of the 
unknown ao- The solution will have the following form: 


= Mi 


S fc -(r)£X*a 0 


= M fc S fc - [M fc (<^ c )rxS] ao. 


(37) 

(38) 


If ( n c )k is the number of degrees of constraint for the general joint at the tip of chain k , then M* is 
the (n c )k X (n c )k transformation matrix which solves for h%. By carefully considering the rank of the 
coefficient matrix, this general solution can still be used for a chain in a singular position or a chain 
with less than six original degrees of freedom [11]. This solution procedure requires 0[(n c )^] scalar 
operations. 


Note that if chain k is rigidly grasping the reference member, then the constraint space for this 
general joint, ( 4 > c ) k , is the 6 X 6 identity matrix. In this case, also note that a c k and h* are identically 
zero for each chain. If chain k has six degrees of freedom and is not in a singular position, then M*. 
will be exactly equal to A*,, the operational space inertia matrix for chain k, and the solution for hj. 
will be: 

h* = A* [Sjt — Xq a 0 ] • (39) 

This solution corresponds to the simple example discussed at the beginning of this section, but now 
expressed in local coordinates. 
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Given the general solution for h£ in Eq. (38), the force vector, f*, may now be written: 


(4>)k h fc + ( 4> c ) k h£, 

(40) 

[(<f>)k + ( <j> c )k M* S*] - [( <j> c )k Mi ( 4> c )l X£] a 0 , 

(41) 

Pjt - 14*0, 

(42) 


where P* and R* are of dimension 6x1 and 6x6, respectively, and both may be computed from 
known quantities. We now have an explicit equation relating the force vector exerted by chain k 
and the spatial acceleration of the reference member. We may combine this information with the 
dynamic equation for the reference member to solve for ao explicitly. 


The dynamic equation for the reference member given in Eq. (16) may be rewritten as follows: 

m 

£(Xo fc ) r f fc + go = Ioao + b 0 . (43) 

k=i 

where f* is the spatial force exerted by chain k on the reference member, expressed in the coordinate 
frame of the general joint at the chain tip. If the expression for f* in Eq. (42) is used in Eq. (43), we 
obtain: 


“ R* a o) — Iq ao + bo — go* 


k= 1 


(44) 


Summing like terms, we may write: 



£(x£) t r* 


1 


a 0 


or, expanding R*, 


m 


[£(X$) r P*-bo + goj , 


(45) 


Jo + £(X*) T (^)* M* (4>')l (X$) 

L k = i 

In Eq. (46), the 6 X 1 spatial acceleration vector of the reference member, ao, is the only unknown. 
We may find a solution for ao from the given set of linear algebraic, equations using any efficient 
linear system solver. Because the required system solution always involves a 6 x 6 coefficient matrix, 
the computational cost of solving for ao is constant. 


ao = 


]T(X$) T P fc -bo + go 


fc=i 


(46) 


The coefficient matrix of a 0 in Eq. (46) represents the combined inertial properties of all the chains 
and the reference member. The inertial properties of each chain are first projected to the tip of that 
chain by computing the inverse operational space inertia matrix, A* 1 . Along the free directions of 
the general joint which connects the chain tip and the reference member, the projected inertia of the 
chain is not felt by the reference member. Along the constrained directions of the joint, however, the 
corresponding components of A* 1 are reflected across to the reference member. These components, 
spatially transformed to the coordinate origin of frame 0, are combined with the spatial inertia of 
the reference member, Io. This combination represents the effective operational space inertia of the 
simple closed-chain mechanism defined at the coordinate origin of frame 0. It is the effective inertia 
“felt” by the reference member in the present state. The bracketed term on the right side of Eq. (46) 
represents the spatial forces which act on the reference member at the given instant. 

Once the spatial acceleration of the reference member is known, the spatial force vector applied by 
chain k to the reference member is defined by Eq. (42). That is, with a 0 given, we may compute f k 
as follows: 


82 


(47) 


f* = P* — R* a o* 

Recall that i k is defined with respect to the coordinate frame of the general joint between chain k 
and the reference member. The explicit knowledge of f* allows us to treat chain k as an independent 
chain with a known tip force. We may now solve for the general closed- chain joint accelerations for 
chain k using Eq. (10), repeated here for convenience: 

<1* = (qAr)open “ &k ft- ( 48 ) 

The application of Eq. (48) to every actuated chain in the simple closed-chain mechanism results in a 
complete solution to the Direct Dynamics problem for this robotic system. The next state positions 
and velocities may be computed by integrating the appropriate quantities for each chain and the 
reference member. As discussed in [11], small amounts of negative position and rate feedback may 
be employed to counteract the drift which is a result of the integration process, and which would 
violate the kinematic constraints. 

The algorithm developed here for simple closed-chain mechanisms may be presented as a series of 
five steps. They are as follows: 

1. The Open Chain Solution, 

2. Calculation of the Spatial Acceleration of the Reference Member, 

3. Calculation of the Spatial Chain Tip Forces, 

4. Calculation of the Closed-Chain Joint Accelerations, 

5. Integration for the Next State. 

The fundamental computations required in each of these steps are summarized in Table 1. In Step 1, 
the Direct Dynamics problem is solved for each chain of the mechanism assuming that the reference 
member has been removed and each chain is in an open, unconstrained state. The general open-chain 
acceleration vectors, (qfc)open and (xfc)open, are computed for each chain, along with the position- 
dependent matrices, and A ^ 1 . In Step 2, Eq. (45) is used to find an explicit solution for ao, 
the spatial acceleration of the reference member, via linear system solution. The quantities (Nl k S k ) 
and [Mfc X§], required for both P* and R*, are computed in the determination of the explicit 
relationship between h£ and a 0 . This relationship is found by linear system solution using Eq. (36), 
with the solution taking the form of Eq. (38). In Step 3, this solution is used in Eq. (47) to solve 
for the spatial force vector exerted on the reference member by each chain. In Step 4, the general 
closed-chain joint accelerations are computed for each chain using Eq. (48), given the spatial tip force 
vector. In Step 5, the appropriate rates and accelerations are integrated to obtain the next state 
positions and rates for the system. 

Note that the first step may be carried out for all chains in parallel, if enough processors are available 
(one per chain). Once the second step is complete and ao is known, the third, fourth, and fifth 
steps may also be carried out for all chains simultaneously. Thus, taking advantage of the structural 
parallelism inherent in the simple closed-chain system has led to parallelism in the computational 
structure of the simulation algorithm. 

V. Computational Requirements 

We will now consider the computational requirements of the dynamic simulation algorithm for simple 
closed-chain mechanisms. First, the number of scalar operations required for each chain of the 
mechanism will be tabulated, followed by the number of operations required to compute the spatial 
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Table 1: Dynamic Simulation Algorithm for Simple Closed-Chain Mechanisms 


Given: a c k , h fc , ( <f> c ) k , ( 4>)k , and with 

Xq, bo, go, Co determined from the reference member state; 

Step 1. Compute (qfc) ope „, (xfc) 0 pen, &k, and A* 1 ; k = l,...,m. 

Step 2. Solve for ao: 

* m m 

Io + E( X 5) TR fe = 5Z(Xo) T P fc — b 0 -b go , 

fc=i J Lfc=i 

with 

P k = [(<f>)kh k + (4> e ) k M k S k ], 

R* = [(<A c )fcM*(* c )£x£], 

Sfc = <4 - ml [Co* - (x^open + Aj" 1 (*)fc hfc] , 

and where (M* St) and Jm* (<f> c )l XqJ are determined by the solution of: 

(M* ') h% = [ml a; 1 (* c )t] hfc = S* - [ml X$] ao. 

Step 3. Solve for ft; k = 1, . . . , m: 

Cb = Pfc-Rfca 0 . 

Step 4. Solve for qt; k = 1, . . . , m: 
q* = (qjfc)open - ffc. 

Step 5. Integrate to obtain the next state positions and rates for the system. 


84 



Table 2: Computations Per Chain in the Simple Closed-Chain Dynamic Simulation Algorithm 





#Mult. 

#Add. 

Calculation 

#Mult. 

#Add. 

(N = 6, n c = 3) 

(JV = 6, n c = 3) 

q open > X 0 p e n 

2507V - 182 

2207V - 167 

1318 

1153 

n, a- 1 

4007V - 621 

3207V - 528 

1779 

1392 

P,R 

g n c 3 + 6%n c 2 + 5^n c + 26 

g n c 3 + 6n c 2 + |n c + 10 

105 

71 

X T P,X T R 

36n c + 20 

36n c - 24 

128 

84 

f 

36 

36 

36 

36 

q 

67V 

67V 

36 

36 

Total: 

6567V - 767 

546 jV - 659 

3402 

2772 


+ (|n c 3 + 6^ Tie 2 + 41 ^ n c + 46) 

+ ( g n c 3 -1- 6n c 2 + 36|n c - 14) 




acceleration of the reference member. The computational complexity of the complete algorithm will 
then be discussed, and the parallel implementation of this algorithm will be considered. 

Table 2 lists the number of scalar operations (multiplications, additions) required in the simulation 
algorithm for each chain of a simple closed-chain mechanism. The operations are tabulated for the 
case of an N degree- of- freedom serial-link chain with simple revolute and/or prismatic joints only. 
The 0(N) Direct Dynamics algorithm of [5] is used to compute the open-chain terms, q ope n and 
x op en- The 0(N) Force Propagation Method of [11] is used to compute ft and A -1 . All q op€n , x ope n, 
ft, and A - ** 1 are computed in Step 1 of the simulation algorithm. 

In Step 2 of the simulation algorithm, the spatial acceleration of the reference member is calculated 
using Eq. (45). For this task, X r P and X r R must be computed for each chain. The number of 
operations required to compute P, R, X T P, and X T R are also listed in Table 2. In this case, the 
number of operations is a function of the number of degrees of constraint at the general joint between 
the chain tip and the reference member (n c ). This number can never be greater than six. The 
computational complexity of these calculations is 0(nj?) due to the linear system solution required 
in the computation of both P and R (see Table 1). 

The spatial force vector, f, exerted by each chain on the reference member, and the closed-chain joint 
accelerations for the chain, q, are calculated in Steps 3 and 4 of the simulation algorithm, respectively. 
The appropriate equations are given in Table 1. The operations required to calculate these vectors 
complete the table. The operations required for the special case of N = 6 and n c = 3 are given in 
the last two columns of Table 2. This value of n c could correspond to a hard point contact between 
a manipulator tip and surface of a load object when the tip is not slipping. 

Given the computations required for each individual chain, the number of scalar operations needed 
to compute the spatial acceleration of the reference member, a 0 , is given in Table 3. Equation (45) 
is used to obtain the solution, which requires O(m) spatial additions and a single 6x6 symmetric 
linear system solution. Thus, the number of operations required for ao is a function only of m, the 
number of chains in the simple closed-chain mechanism. The example of three chains (m = 3) is 
given in the last two columns of this table. 

To determine the total number of scalar operations required to simulate the entire simple closed- 
chain mechanism, the number of operations required for a single chain is simply multiplied by m, 
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Table 3: Computations for the Spatial Acceleration of the Reference Member 


Calculation 

#Mult. 

#Add. 

(to = 3) 

#Add. 
(to = 3) 

ao 

86 

27 to + 71 

86 

152 


the number of chains, and added to the computations required for ao for the same number of chains. 
Thus, the computational complexity of the complete simulation algorithm is O(mN) for a given value 
of n c < 6. 

The total computational complexity discussed in the previous section only considered the execution of 
the simulation algorithm on a single processor. In order to speed up the simulation, parallel processing 
may be investigated. If a single processor is used for the entire system of m chains, the computational 
complexity of the simulation algorithm is 0(mN) for a given n c < 6. Given ao, all computations for 
each chain may be carried out independently. Thus, if m processors are available, the computational 
tasks associated with each chain may be performed in parallel, and the computational complexity 
of the operations required for the m chains may be reduced to O(N). Of course, the computations 
required to compute ao must also be considered. These operations may also be implemented in 
parallel on the m available processors. Equation (45) requires 0(m) spatial additions to compute a 0 . 
On (m + l)/2 parallel processors, this task may be carried out in 0(log 2 m+l) operations by using 
the recursive doubling approach [12]. Thus, on m parallel processors, the computational complexity 
of the entire dynamic simulation algorithm may be reduced to O(N) + 0(log 2 m + l). 

VI. Summary and Conclusions 

In this paper, a general and efficient dynamic simulation algorithm for simple closed-chain mechanisms 
was derived. The algorithm is applicable to both Type 0 and Type 1 mechanisms. Both types of 
mechanisms are modelled in a convenient and general manner through the use of the general joint 
concept. The operational space inertia matrix of each chain is used to project the dynamic properties 
of the chain to its tip where it is coupled to the reference member. By combining the operational 
space inertia of each chain with the model of the general joint at each chain tip, a solution may be 
found for the spatial acceleration of the reference member and the spatial force vector exerted on it 
by each chain. Once the force vectors are completely defined, the system is effectively decoupled, 
and the joint accelerations for each chain may be computed separately. 

The computational complexity of the new simulation algorithm is 0(mN) when implemented on a 
single processor. The linear dependence on iV is a significant improvement over previous simulation 
algorithms such as that presented in [1], The computational complexity of the new algorithm may 
be further reduced to 0(N ) + O(log 2 m+1) if it is implemented on m processors in parallel. 
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